#include "ekf.h"
#include "output/output.h"

#ifndef EPS
#define EPS 1e-12
#endif

using namespace Filtering;

GeoPoint ExtendedKalmanFilter::Predict(const GeoPoint& gps_point)
{
    double prev_dis = Eigen::Vector2d(gps_point.x - x_.x(), gps_point.y - x_.y()).norm();
    if (prev_dis < EPS)
    {
        return gps_point;
    }

    Control u;
    u.dt()  = (gps_point.timestamp - last_p.timestamp) / 1000.0;
    u.v() = gps_point.speed;
    x_ = predict(sys_, u);
    last_p = gps_point;
    
    GeoPoint predict_geo = gps_point;
    predict_geo.x = x_.x();
    predict_geo.y = x_.y();
    predict_geo.bearing = x_.theta();
    predict_points.push_back(predict_geo);
    return predict_geo;
}

bool ExtendedKalmanFilter::Update(const GeoPoint &gps_point)
{
    Measurement measure;
    measure.x() = gps_point.x;
    measure.y() = gps_point.y;
    measure.theta() = gps_point.bearing;
    x_ = update(pm_, measure);
    return true;
}

void ExtendedKalmanFilter::Print(DbgFrame* dbg_frame)
{
  dbg_frame->DrawTrcPoints(predict_points, cv::Scalar(0, 255, 0));
}